#pragma once

#include "Assert.h"
#include "CoreApi.h"
#include "MathUtils.h"

using namespace loco;

void test_Quaternion()
{
	// constructor
	{
		Quaternion q1(1.0f, 2.0f, 3.0f, 4.0f);
		ASSERT(q1.x == 1.0f);
		ASSERT(q1.y == 2.0f);
		ASSERT(q1.z == 3.0f);
		ASSERT(q1.w == 4.0f);

		Quaternion q2(Vector3::unit_z, HALF_PI);
		ASSERT(Vector3::near_equal(q2.axis(), Vector3::unit_z));
		ASSERT(loco::near_equal(q2.angle(), HALF_PI));

		Vector3 euler(45.0f, 45.0f, 10.0f);
		Matrix4 rotation_mat = rotation_matrix_from_euler(euler);

		Quaternion q3(rotation_mat);
		Quaternion q4(euler);
		ASSERT(Quaternion::near_equal(q3, q4));

		euler = Vector3(90.0f, 0.0f, 0.0f);
		Quaternion q5(euler);
		ASSERT(Vector3::near_equal(q5.axis(), Vector3::unit_z));
		ASSERT(loco::near_equal(q5.angle()*RAD_TO_DEG, 90.0f));
	}

	// near_equal
	{
		Quaternion q1(1, 2, 3, 4);
		Quaternion q2 = q1;
		ASSERT(Quaternion::near_equal(q1, q2));
		q2.z += 0.5f * DELTA;
		ASSERT(Quaternion::near_equal(q1, q2));
		q2.z +=DELTA;
		ASSERT(!Quaternion::near_equal(q1, q2));
	}

	// operator*
	{
		Quaternion q1(Vector3(45, 10, 20));
		Quaternion q2(Vector3(45, 0, 0));
		Quaternion q3(Vector3(0, 10, 0));
		Quaternion q4(Vector3(0, 0, 20));
		Quaternion q5 = q2 * q3 * q4;

		ASSERT(Quaternion::near_equal(q1, q5));
	}

	// euler
	{
		Vector3 euler(90, 0, 0);
		Quaternion q(euler);
		Vector3 euler_result = q.euler();
		ASSERT(Vector3::near_equal(euler, euler_result));
	}

	// rotate
	{
		Quaternion q;

		q = Quaternion(Vector3(90, 0, 0));
		ASSERT(Vector3::near_equal(q.rotate(Vector3::unit_x), Vector3::unit_y));

		q = Quaternion(Vector3(0, 90, 0));
		ASSERT(Vector3::near_equal(q.rotate(Vector3::unit_x), Vector3::unit_z));
	}


	// inverse
	{
		Vector3 euler(5.0f, 45.0f, 27.0f);
		Quaternion q(euler);
		ASSERT(Quaternion::near_equal(q * q.inverse(), Quaternion::identity));
	}
}